#include "info_process.hpp"

 Motor_Info_Press::Motor_Info_Press()
    {
        for(int i=0;i<4;i++)
        {
           lun_speed_set[i]=0;
           lun_speed_cur[i] =0; 
           zhuan_speed_set[i] = 0;
           zhuan_speed_cur[i] = 0;
           angle_cur[i] = 0; 
           angle_set[i] = 0;
        }
    }
 float Motor_Info_Press::convert_rmp_to_ms(int sp)
 {
    float line_v;
    float temp;
    temp=(float)sp/speed_change;
    line_v=temp*2.0*WHEEL_RADIUS*PI/60.0;
    return line_v;
 }

int Motor_Info_Press::convert_ms_to_rmp(float fsp) 
  {
    int spd_rmp;
    spd_rmp=(int)(fsp*60.0*speed_change/(2.0*WHEEL_RADIUS*PI));
    return spd_rmp;
  } 

void Motor_Info_Press::Driver_cal_real(float lx,float ly,float wmga)
{
   int nov_flag;
   float liney=0.0;
   float linex=0;
   if(wmga>0.6)wmga=0.6;
   if(wmga<-0.6)wmga=-0.6;
   linex=lx;        //+0.001;
   liney=ly;
  //驱动算法矩阵B=AX，计算结果为矢量                 
  double vfr_x = linex + RX * wmga;
  double vfr_y = liney + RY * wmga;
  double vfl_x = linex - RX * wmga;
  double vfl_y = liney + RY * wmga;
  double vrl_x = linex - RX * wmga;
  double vrl_y = liney - RY * wmga;
  double vrr_x = linex + RX * wmga;
  double vrr_y = liney - RY * wmga;

  //矢量分解，得到速度值和角度                    
  // angle_c[1] = atan2(vfl_y, vfl_x) * 180 / M_PI; 
  // angle_c[2] = atan2(vfr_y, vfr_x) * 180 / M_PI;
  // angle_c[0] = atan2(vrl_y, vrl_x) * 180 / M_PI;
  // angle_c[3] = atan2(vrr_y, vrr_x) * 180 / M_PI;

  // rpm_c[1] = calcDistance(vfl_x, vfl_y);
  // rpm_c[2] = calcDistance(vfr_x, vfr_y);
  // rpm_c[0] = calcDistance(vrl_x, vrl_y);
  // rpm_c[3] = calcDistance(vrr_x, vrr_y);
  angle_c[2] = atan2(vfl_y, vfl_x)  ; 
  angle_c[1] = atan2(vfr_y, vfr_x) ;
  angle_c[3] = atan2(vrl_y, vrl_x)  ;
  angle_c[0] = atan2(vrr_y, vrr_x)  ;

  rpm_c[2] = calcDistance(vfl_x, vfl_y);
  rpm_c[1] = calcDistance(vfr_x, vfr_y);
  rpm_c[3] = calcDistance(vrl_x, vrl_y);
  rpm_c[0] = calcDistance(vrr_x, vrr_y);
  //最合适执行的角度与速度
  for( int i = 0; i < 4; i++ )
  {
    //速度上限
    if( rpm_c[i] >= 1.5 )
    {
      rpm_c[i] = 1.5;
    }

    //判断是否需要反转
    if( ( angle_c[i] >= (M_PI/2.0) ) || ( angle_c[i] <= -(M_PI/2.0) ) )
    {
      nov_flag = 1;
    }
    else
    {
      nov_flag = 0;
    }

    //得到最合适角度与速度
    if( nov_flag == 1 )
    {
      angle_b[i] = angle_c[i] - ( sgn(angle_c[i]) * M_PI );
      rpm_b[i]   = -rpm_c[i];
    }
    else
    {
      angle_b[i] = angle_c[i];
      rpm_b[i]   = rpm_c[i];
    }

  }
  
}

float Motor_Info_Press::convert_yaogan_wmga( int16_t w)
{
  float yg,wmga;
  yg=(float)w;
  wmga= wmga_max *yg/128.0;
  return wmga;
}
////将摇杆值转换成线速度 最大1m/s
float Motor_Info_Press::convert_yaogan_line(int16_t v)
{
  float vt,tempf;
  tempf =(float) v;
  vt= v /128.0*0.5;
  return vt;
}

 void Motor_Info_Press::run_set_angle(void)
 {
    int sp_temp;
    lun_enable=1;
    for(int i=0;i<4;i++)
        {
          sp_temp=(angle_set[i]-angle_cur[i]);  
          if(abs(sp_temp)>500)lun_enable=0;

        if((sp_temp<460)&&(sp_temp>-460))sp_temp=0;
        if(sp_temp>4500)sp_temp=4500;
        if(sp_temp<-4500)sp_temp=-4500;
        zhuan_speed_set[i]=sp_temp;
        }
 }
 void Motor_Info_Press::convert_line_real(void)
 {
  //  for(int i=0;i<4;i++)
  //  {
      lun_speed_set[0]=convert_ms_to_rmp(rpm_b[3]);
      angle_set[0]=(int)(angle_b[3]*32768.0/M_PI);
 //   }
      lun_speed_set[1]=convert_ms_to_rmp(rpm_b[2]);
      angle_set[1]=(int)(angle_b[2]*32768.0/M_PI);
       lun_speed_set[2]=convert_ms_to_rmp(rpm_b[1]);
      angle_set[2]=(int)(angle_b[1]*32768.0/M_PI);
       lun_speed_set[3]=convert_ms_to_rmp(rpm_b[0]);
      angle_set[3]=(int)(angle_b[0]*32768.0/M_PI);
 }

